/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "fc_sensors.h"
#include "filter.h"
#include "fc_error.h"

#define DBG_TAG "acc"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

ZINO_CONFIG_REGISTER(ZeroOffset_t, ZeroSave, 2, 1);

biquadFilter_t acce_lpf1[3];
biquadFilter_t acce_lpf2[3];
bool acc_lpf_init = false;
bool acc_use_lpf = true;
// bool ZeroSaveFlag()
// {
//     return (ZeroSave()->accoffset.x && ZeroSave()->accoffset.y && ZeroSave()->accoffset.z && ZeroSave()->gyrooffset.x && ZeroSave()->gyrooffset.y && ZeroSave()->gyrooffset.z);
// }
// void sensordata_save_acc_bias(acc_t *acc)
// {
//     ZeroSave()->accoffset.x = acc->acc_offset.zero.x;
//     ZeroSave()->accoffset.y = acc->acc_offset.zero.y;
//     ZeroSave()->accoffset.z = acc->acc_offset.zero.z;

//     zino_config_save_all();
// }
// void sensordata_get_save_acc_bias(acc_t *acc)
// {
//     acc->acc_offset.zero.x = ZeroSave()->accoffset.x;
//     acc->acc_offset.zero.y = ZeroSave()->accoffset.y;
//     acc->acc_offset.zero.z = ZeroSave()->accoffset.z;
// }
void sensor_acc_lpf_init(void)
{
    if(ZeroSave()->acc_cutoff == 0)
        ZeroSave()->acc_cutoff = ACC_LPF_CUTOFF;
    if(ZeroSave()->acc_cutoff > 500)
        ZeroSave()->acc_cutoff = 500;

    LOG_I("ACC LPF CUTOFF %dHz\n", ZeroSave()->acc_cutoff);
    for (uint8_t i = 0; i < 3; i++)
    {
        biquadFilterInitLPF(&acce_lpf1[i], ZeroSave()->acc_cutoff, 1000.f);
        // biquadFilterInitLPF(&acce_lpf2[i], 50, 1000.f);
    }
}
void AccStartCalibration(void)
{
    fc_sensor_data.acc.acc_offset.isdatacalibrated = false;
    fc_sensor_data.acc.acc_offset.samplesize = ACC_GYRO_CALIBRATE_SAMPLE;
    FC_ERROR_SET(FC_IMU_CALIBRATING);
}
void sensor_acc_calibrate(acc_t *acc)
{
    if (acc->acc_offset.samplesize != 0)
    {
        if (acc->acc_offset.samplesize == ACC_GYRO_CALIBRATE_SAMPLE)
        {
            for (int i = 0; i < 3; i++)
            {
                acc->acc_offset.zero.axis[i] = 0;
            }
        }
        for (uint8_t i = 0; i < 3; i++)
        {
            acc->acc_offset.zero.axis[i] += acc->accRaw.axis[i];
        }

        if (acc->acc_offset.samplesize == 1)
        {
            for (uint8_t i = 0; i < 3; i++)
            {
                acc->acc_offset.zero.axis[i] /= ACC_GYRO_CALIBRATE_SAMPLE;
                //**SAVING ACC BIAS */
                ZeroSave()->accoffset.axis[i] = acc->acc_offset.zero.axis[i];
                FC_ERROR_RESET(FC_IMU_CALIBRATING);
            }
            acc->acc_offset.isdatacalibrated = true;
            ZeroSave()->acc_calibrated = true;
            ZeroSave()->flag = ZERO_SAVED_FLAG;
            zino_config_save_all();
        }
        acc->acc_offset.samplesize--;
        // rt_kprintf("ACC CALIBRATING %d\n", acc->acc_offset.samplesize);
    }
}
void sensor_acc_process(struct rt_sensor_data *sensor)
{
    if (sensor->type == RT_SENSOR_CLASS_ACCE)
    {
        acc_t *acc = &fc_sensor_data.acc;

        /**get raw data */
        acc->accRaw.x = sensor->data.acce.x;
        acc->accRaw.y = sensor->data.acce.y;
        acc->accRaw.z = sensor->data.acce.z;

        if (!ZeroSave()->acc_calibrated || !acc->acc_offset.isdatacalibrated)
        {
            //**First boot calibration */
            if (!ZeroSave()->acc_calibrated)
            {
                ZeroSave()->accoffset.x = 0;
                ZeroSave()->accoffset.y = 0;
                ZeroSave()->accoffset.z = 0;
            }
            if (acc->acc_offset.samplesize == 0)
            {
                AccStartCalibration();
            }
            /**perform calibration */
            sensor_acc_calibrate(acc);
        }

        if (!acc_lpf_init)
        {
            acc_lpf_init = true;
            sensor_acc_lpf_init();
        }

        acc->accdata.x = (float)(acc->accRaw.x - ZeroSave()->accoffset.x) / 1000.f;
        acc->accdata.y = (float)(acc->accRaw.y - ZeroSave()->accoffset.y) / 1000.f;
        acc->accdata.z = (float)(acc->accRaw.z - ZeroSave()->accoffset.z) / 1000.f + 1.0f;

        for (uint8_t i = 0; i < 3; i++)
        {
            if (acc_use_lpf)
            {
                acc->acclpf.axis[i] = biquadFilterApply(&acce_lpf1[i], acc->accdata.axis[i]);
                // acc->acclpf.axis[i] = biquadFilterApply(&acce_lpf2[i], acc->acclpf.axis[i]);
            }
            else
            {
                acc->acclpf.axis[i] = acc->accdata.axis[i];
            }
        }
    }
}
void acc_trace(fc_sensor_data_t *sensor, zdrone_state_t *state, float dt)
{
#define ACC_NOSISE 0.004f
    // fc_sensor_data.acc.accSum.x += ((ax > -ACC_NOSISE && ax < ACC_NOSISE) ? 0 : ax); // 1/2*a*t^2
    // fc_sensor_data.acc.accSum.y += ((ay > -ACC_NOSISE && ay < ACC_NOSISE) ? 0 : ay);
    // fc_sensor_data.acc.accSum.z += ((az > -ACC_NOSISE && az < ACC_NOSISE) ? 0 : az);

    // {
    // fc_sensor_data.acc.accSum.x = 0;
    // fc_sensor_data.acc.accSum.y = 0;
    // fc_sensor_data.acc.accSum.z = 0;
    // fc_sensor_data.acc.sumCount.x = 0;
    for (uint8_t i = 0; i < 3; i++)
    {
        if (FC_ERROR_CHECK(FC_IMU_CALIBRATING))
        {
            sensor->acc.sumCount.axis[i] = 0;
            sensor->acc.accSum.axis[i] = 0;
            sensor->acc.sumTimer.axis[i] = 0;
        }
        else
        {
            // state->acc.axis[i]=constrainf(state->acc.axis[i],-1.0f,1.0f);
            sensor->acc.sumCount.axis[i]++;
            sensor->acc.accSum.axis[i] += ((state->acc.axis[i] > -ACC_NOSISE && state->acc.axis[i] < ACC_NOSISE) ? 0 : state->acc.axis[i]);
            sensor->acc.sumTimer.axis[i] += dt;
        }
    }
}
